// mdvctrl - A video-driven controller for MD-25 motor drivers
// Copyright (C) 2010  Nuno J. Silva
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, version 3 of the License.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

// This module provides a partial interface for the MD25 Serial Port
// interface, as described on the web at
// http://www.robot-electronics.co.uk/htm/md25ser.htm


// Abstract the binary values of some parts of the commands.
#define PREAMBLE "\x00"
#define LEFT_ENGINE_SET_SPEED "\x31"
#define RIGHT_ENGINE_SET_SPEED "\x32"

// This only implements two commands: 
//  - setting the left engine speed, and
//  - setting the right engine speed.
//
// As documented, both commands have a byte-wide argument (the speed
// value), and must be preceded by a null (zero-filled) "sync byte".


// Some details on the controller card:
//
// - Unless a specific command is issued, the engines are halted two
//   seconds after the last command.
//
// - By default, a speed is a value between 0 and 255:
//    - 128 is "stop", no speed at all;
//    - <128 moves backward
//    - >128 moves forward

#include <termios.h>
#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>

#include "fatal.h"

#include <stdio.h>

// Each function gets a file descriptor to which the commands will be
// written.


// Open and configure a serial port
int open_md25 (const char * filename) {
  int fd = open (filename, O_RDWR | O_NOCTTY | O_NONBLOCK, 0);
  struct termios * robot = (struct termios *) malloc (sizeof (struct termios));

  if (robot == NULL)
    fatal ("virtual memory exhausted.");

  if (tcgetattr (fd, robot) == -1) {
    int error = errno;
    fatal (strerror (errno));
  }

  // Set the I/O speed for the serial port adapter (which depends on
  // the jumper settings of the controller card, see
  // http://www.robot-electronics.co.uk/htm/md25tech.htm)
  cfsetispeed (robot, B38400);
  cfsetospeed (robot, B38400);

  if (tcsetattr (fd, TCSANOW, robot) == -1) {
    int error = errno;
    fatal(strerror(errno));
  }
     
  return fd;
}

// Writes the required "sync byte"
void write_preamble (int fd) {
  write (fd,PREAMBLE,1);
}

// Send a command to set the left engine speed
void leftmove (int fd, unsigned char speed) {
  write_preamble (fd);
  write (fd, LEFT_ENGINE_SET_SPEED, 1);
  write (fd, &speed, 1);
}

// Send a command to set the right engine speed
void rightmove (int fd, unsigned char speed) {
  write_preamble (fd);
  write (fd, RIGHT_ENGINE_SET_SPEED, 1);
  write (fd, &speed, 1);
}

// Move forward (set both the left and the right speed)
void move (int fd, unsigned char speed) {
  leftmove (fd, speed);
  rightmove (fd, speed);
}

// Set both speeds with different values (in order to steer)
void steermove (int fd, unsigned char speed, int difference) {
  // Truncate speeds when they would get below 128 (i.e. when they
  // would make the engine move backward)
  if (speed - difference < 127) {
    difference = speed - 127;
  }
  if (speed + difference < 127) {
    difference = - (speed - 127);
  }
  leftmove (fd, speed + difference);
  rightmove (fd, speed - difference);
}


// Open a file to write commands there, but don't set special
// attributes. Useful for logging.
//
// If no filename is given, it uses stdout.
int open_md25_log (const char * filename) {
  int robotfd;
  if (filename != NULL) {
    robotfd = open (filename, O_RDWR);
    if (robotfd == -1) {
      int error = errno;
      fatal (strerror (errno));
    }  
  } else {
    robotfd = STDOUT_FILENO;
  }
  return robotfd;
}
